/* AUTHORS OF LINEFOLLOW.c -> SAMUEL MCNAMARA & KILLIAN MURPHY 
   ALL CODE PRODUCED BY SAMUEL MCNAMARA UNLESS OTHERWISE STATED */

/* INCLUSIONS */
#include "LPC17xx.h"
#include "lpc17xx_uart.h"
#include "lpc17xx_pinsel.h"
#include "LINEFOLLOW.h"

/* DEFINITIONS */

// UART MACROS (AUTHOR -> SAMUEL MCNAMARA)
#define UART_SEND_B(s,n)   UART_Send((LPC_UART_TypeDef *)LPC_UART3, s, n, BLOCKING)
#define UART_SEND_NB(s,n)   UART_Send((LPC_UART_TypeDef *)LPC_UART3, s, n, NONE_BLOCKING)
#define UART_RCV_B(s,n)   UART_Receive((LPC_UART_TypeDef *)LPC_UART3, s, n, BLOCKING)
#define UART_RCV_NB(s,n)   UART_Receive((LPC_UART_TypeDef *)LPC_UART3, s, n, NONE_BLOCKING)

// POLULU SERIAL SLAVE COMMANDS (AUTHOR -> SAMUEL MCNAMARA)
static uint8_t CMDS[6] = {0x86, 0x87, 0xB5,
			  0xBA, 0xBB, 0xBC};
#define CMD_RAWS       CMDS
#define CMD_CALS       (CMDS+1)
#define CMD_RESET      (CMDS+2)
#define CMD_AUTO       (CMDS+3)
#define CMD_STARTPID   (CMDS+4)
#define CMD_STOPPID    (CMDS+5)

/* GLOBALS (AUTHOR -> KILLIAN MURPHY) */
static calib_status CALIBRATION_STATUS;
LineFollowCfg CFG = {0x1F, 0x01, 0x14,
		     0x03, 0x02};
uint8_t FOLLOWING = 0;

SensorVals s = {RAW, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000};

/* FUNCTION BODIES */
// AUTHOR -> SAMUEL MCNAMARA
void LINE_ResetCalibration(void){
  UART_SEND_B(CMD_RESET, 1);
  CALIBRATION_STATUS = RAW;
}

// AUTHOR -> SAMUEL MCNAMARA
void LINE_AutoCalibrate(void){
  UART_SEND_B(CMD_AUTO, 1);
  uint8_t rcv = 0;
  UART_RCV_B(&rcv, 1);
  CALIBRATION_STATUS = CALIBRATED;
}

// AUTHOR -> SAMUEL MCNAMARA
SensorVals* LINE_GetSensors(SensorVals* sv){
  uint8_t data[10];
  UART_SEND_B((CALIBRATION_STATUS?CMD_CALS:CMD_RAWS),1);
  UART_RCV_B(data, sizeof(data));

  sv->status = CALIBRATION_STATUS;;
  sv->PC0 = ((uint16_t)(data[1] << 8))|(data[0]);
  sv->PC1 = ((uint16_t)(data[3] << 8))|(data[2]);
  sv->PC2 = ((uint16_t)(data[5] << 8))|(data[4]);
  sv->PC3 = ((uint16_t)(data[7] << 8))|(data[6]);
  sv->PC4 = ((uint16_t)(data[9] << 8))|(data[8]);
  return sv;
}

// AUTHOR OF LINE_StartFollow -> KILLIAN MURPHY
void LINE_StartFollow(LineFollowCfg* fc){
  FOLLOWING = 1;
  uint32_t i = 4000000;
  uint8_t cmd_string[6] = { *CMD_STARTPID,
			    fc->FollowSpeed,
			    fc->P_Factor,
			    fc->P_Quotient,
			    fc->D_Factor,
			    fc->D_Quotient, };
  UART_SEND_B(cmd_string, 6);

  while (FOLLOWING) {
    LINE_GetSensors(&s);
    if(!(s.PC0 > 0x50 ||
	 s.PC1 > 0x50 ||
	 s.PC2 > 0x50 ||
         s.PC3 > 0x50 ||
         s.PC4 > 0x50)){
      LINE_EndFollow(0x00);
      FOLLOWING = 0;
      break;
      }
    while(i--);
    i = 3000000;
  }
}

// AUTHOR -> SAMUEL MCNAMARA
void LINE_EndFollow(uint8_t spd){
  UART_SEND_B(CMD_STOPPID,1);
  uint8_t buf[4] = {0xC1, spd, 0xC5, spd};
  UART_SEND_B(buf, 4);  
}
